#ifndef __FOLLOW_H__
#define __FOLLOW_H__

#include "my_lib/steer.h"
#include "my_lib/cmd_analyse.h"


//default angle pid define
#define PointKp   0.0006f
#define PointKi   0.0f
#define PointKd   0.0005f
#define PointOutMax  0xffffff   //不限制
#define PointBiasAllow 0.0f


#define PointKp_START   0.0001f
#define PointKd_START   0.00001f

#define PointKp_FUN2   0.0004f
#define PointKd_FUN2   0.002f

//PID控制结构体
typedef struct 
{
    uint8_t PIDCtrl_Flag;
    float Kp;
    float Ki;
    float Kd;
    float Bias;
    float LastBias;
    float BiasSum;
    float Target;
    float Present;
    float Output;
    float OutLimit;

    float BiasAllow;  //误差允许(绝对值),避免静态振荡
}PID_Type;

typedef struct
{
    double x_bias;
    double y_bias;
}point_bias_t;

void PointFollowTask(void* param);

void IsPointfollow(uint8_t sta);
void SetPointBias(double x_bias, double y_bias);

#endif


